function [Aeq, beq]= getAbeq(n_seg, n_order, waypoints, ts, start_cond, end_cond)
    n_all_poly = n_seg*(n_order+1);
    %#####################################################
    % p,v,a,j constraint in start, 
    Aeq_start = zeros(4, n_all_poly);
    beq_start = zeros(4, 1);
    % STEP 2.1: write expression of Aeq_start and beq_start
    %
    %
    %
    %
    for i=1:4
        Aeq_start(i,1:8)=get_coeff(n_order,i-1,0);
        beq_start(i)=start_cond(i);
    end
    
    %#####################################################
    % p,v,a,j constraint in end
    Aeq_end = zeros(4, n_all_poly);
    beq_end = zeros(4, 1);
    % STEP 2.2: write expression of Aeq_end and beq_end
    %
    %
    %
    %
    for i=1:4
        Aeq_end(i,end-7:end)=get_coeff(n_order,i-1,ts(end));
        beq_end(i)=end_cond(i);
    end
    
    %#####################################################
    % position constrain in all middle waypoints
    Aeq_wp = zeros(n_seg-1, n_all_poly);
    beq_wp = zeros(n_seg-1, 1);
    % STEP 2.3: write expression of Aeq_wp and beq_wp
    %
    %
    %
    %
    for i=1:n_seg-1
        Aeq_wp(i, 8*i+1:8*(i+1))= get_coeff(n_order,0,0); % 第i+1段的起点
        beq_wp(i)= waypoints(i+1);
    end
    
    %#####################################################
    % position continuity constrain between each 2 segments
    Aeq_con_p = zeros(n_seg-1, n_all_poly);
    beq_con_p = zeros(n_seg-1, 1);
    % STEP 2.4: write expression of Aeq_con_p and beq_con_p
    %
    %
    %
    %
    for i=1:n_seg-1
        Aeq_con_p(i, 8*(i-1)+1:8*i)= get_coeff(n_order,0,ts(i)); % 第i段的起点
        Aeq_con_p(i, 8*i+1:8*(i+1))= -get_coeff(n_order,0,0); % 第i+1段的起点
    end
    
    %#####################################################
    % velocity continuity constrain between each 2 segments
    Aeq_con_v = zeros(n_seg-1, n_all_poly);
    beq_con_v = zeros(n_seg-1, 1);
    % STEP 2.5: write expression of Aeq_con_v and beq_con_v
    %
    %
    %
    %
    for i=1:n_seg-1
        Aeq_con_v(i, 8*(i-1)+1:8*i)= get_coeff(n_order,1,ts(i));
        Aeq_con_v(i, 8*i+1:8*(i+1))= -get_coeff(n_order,1,0);
    end

    %#####################################################
    % acceleration continuity constrain between each 2 segments
    Aeq_con_a = zeros(n_seg-1, n_all_poly);
    beq_con_a = zeros(n_seg-1, 1);
    % STEP 2.6: write expression of Aeq_con_a and beq_con_a
    %
    %
    %
    %
    for i=1:n_seg-1
        Aeq_con_a(i, 8*(i-1)+1:8*i)= get_coeff(n_order,2,ts(i));
        Aeq_con_a(i, 8*i+1:8*(i+1))= -get_coeff(n_order,2,0);
    end
    
    %#####################################################
    % jerk continuity constrain between each 2 segments
    Aeq_con_j = zeros(n_seg-1, n_all_poly);
    beq_con_j = zeros(n_seg-1, 1);
    % STEP 2.7: write expression of Aeq_con_j and beq_con_j
    %
    %
    %
    %
    for i=1:n_seg-1
        Aeq_con_j(i, 8*(i-1)+1:8*i)= get_coeff(n_order,3,ts(i));
        Aeq_con_j(i, 8*i+1:8*(i+1))= -get_coeff(n_order,3,0);
    end
    
    %#####################################################
    % combine all components to form Aeq and beq   
    Aeq_con = [Aeq_con_p; Aeq_con_v; Aeq_con_a; Aeq_con_j];
    beq_con = [beq_con_p; beq_con_v; beq_con_a; beq_con_j];
    Aeq = [Aeq_start; Aeq_end; Aeq_wp; Aeq_con];
    beq = [beq_start; beq_end; beq_wp; beq_con];
end

function c = get_coeff(N, K, t) % 返回N阶多项式求K次导数后的系数 在时间t时的系数
    c=zeros(1,N+1);
    for i=K:N
        c(i+1)=factorial(i)/factorial(i-K)*t^(i-K);
    end
end